"""
1.比赛场景简单示例，包含基本的流程，无法直接使用
2.注意实际情况的障碍区，示例中未展示避障逻辑
3.注意实际情况的区域边界判断，示例中未展示判断逻辑
"""
from rflysim import RflysimEnvConfig,Vel
from rflysim.client import VehicleClient
from rflysim.config import AreaInfo,Position
import time
if __name__ == "__main__":
    # 环境配置
    config = {
        "id": 102,                        # 填入网页中参赛方案对应的方案号
        "config": RflysimEnvConfig(
            "127.0.0.1",              # 填入对应的ip地址
            16001,
            18001
        )
    }


    # 初始化client
    client = VehicleClient(id=config["id"], config=config["config"])
    client.enable_rflysim()

    # 停止仿真
    client.stop()
    # 启动仿真
    client.start()

    # 获取区域信息
    res = client.get_areas()
    picks = {}
    deliverys= {}
    barriers = []
    for id in res:
        # id.name为区域名称
        # pick开头代表取货区
        if "pick" in id.name:
            picks[id.name] = id
        # delivery代表收货区
        elif "delivery" in id.name:
            deliverys[id.name] = id
        # barrier代表障碍区
        else:
            barriers.append(id)

    for id in res:
        print(id.name, id.type, id.nNum, id.nPoints)
    
    # 假设先去pick_up_l区域取货物，示例中选择区域中点为取货点
    pos_pick1 = [picks['pick_up_l'].nPoints[i:i+3] for i in range(0, 12, 3)]
    pick_center_lon1 = sum(p[0] for p in pos_pick1) / len(pos_pick1)
    pick_center_lat1 = sum(p[1] for p in pos_pick1) / len(pos_pick1)
    pick1 = Position(pick_center_lon1,pick_center_lat1,0)
    # pick2
    pos_pick2 = [picks['pick_up_m'].nPoints[i:i+3] for i in range(0, 12, 3)]
    pick_center_lon2 = sum(p[0] for p in pos_pick2) / len(pos_pick2)
    pick_center_lat2 = sum(p[1] for p in pos_pick2) / len(pos_pick2)
    pick2 = Position(pick_center_lon2,pick_center_lat2,0)
    #pick 3
    pos_pick3 = [picks['pick_up_h'].nPoints[i:i+3] for i in range(0, 12, 3)]
    pick_center_lon3 = sum(p[0] for p in pos_pick3) / len(pos_pick3)
    pick_center_lat3 = sum(p[1] for p in pos_pick3) / len(pos_pick3)
    pick3 = Position(pick_center_lon3,pick_center_lat3,0)

    # 避障
    """避障逻辑"""

    # 无人车前往取货点，例如id = 10049的无人车
    vehicle1_id = 10049
    res1 = client.set_tactical_maneuver(vehicle_id=vehicle1_id,position=[pick1],tactical_type=1,speed=100)
    print(res1)
    vehicle2_id = 10050
    res2 = client.set_tactical_maneuver(vehicle_id=vehicle2_id,position=[pick2],tactical_type=1,speed=100)
    print(res2)
    vehicle3_id = 10051
    res3 = client.set_tactical_maneuver(vehicle_id=vehicle3_id,position=[pick3],tactical_type=1,speed=100)
    print(res3)


    # 判断并等待达到取货点
    ret1 = ret2 = ret3 = 1
    while ret1:
        try:
            # 获取res这个机动命令的状态
            status = client.get_command_status(res1)
            # print(status)
            if status.get('status') == 'EXECUTE_SUCCESS':
                ret1 = 0
                res = client.get_vehicle_pos()
                for id,pos in res.items():
                    if id == vehicle1_id:
                        print(f"无人车{id}当前位置：{pos} ({pos.x}, {pos.y}, {pos.z})")
                break
        except Exception: 
            continue
    
    while ret2:
        try:
            # 获取res这个机动命令的状态
            status = client.get_command_status(res2)
            # print(status)
            if status.get('status') == 'EXECUTE_SUCCESS':
                ret2 = 0
                res = client.get_vehicle_pos()
                for id,pos in res.items():
                    if id == vehicle2_id:
                        print(f"无人车{id}当前位置：{pos} ({pos.x}, {pos.y}, {pos.z})")
                break
        except Exception: 
            continue

    while ret3:
        try:
            # 获取res这个机动命令的状态
            status = client.get_command_status(res3)
            # print(status)
            if status.get('status') == 'EXECUTE_SUCCESS':
                ret3 = 0
                res = client.get_vehicle_pos()
                for id,pos in res.items():
                    if id == vehicle3_id:
                        print(f"无人车{id}当前位置：{pos} ({pos.x}, {pos.y}, {pos.z})")
                break
        except Exception: 
            continue
    # 根据get_vehicle_pos获取无人车的位置信息
    res = client.get_vehicle_pos()
    for id,pos in res.items():
        if id == vehicle1_id:
            print(f"无人车当前位置：{pos} ({pos.x}, {pos.y}, {pos.z})")
        elif id == vehicle2_id:
            print(f"无人车当前位置：{pos} ({pos.x}, {pos.y}, {pos.z})")
        elif id == vehicle3_id:
            print(f"无人车当前位置：{pos} ({pos.x}, {pos.y}, {pos.z})")

    # 与区域位置判断，判断无人车是否驶入该范围
    # """
    # 判断逻辑
    # """

    # 假设delivery_l为收货区，示例中选择区域中点为收货点，实际情况自行确定
    pos_del = [deliverys['delivery_l'].nPoints[i:i+3] for i in range(0, 12, 3)]
    del_center_lon = sum(p[0] for p in pos_del) / len(pos_del)
    del_center_lat = sum(p[1] for p in pos_del) / len(pos_del)
    delivery = Position(del_center_lon,del_center_lat,0)

    # 避障
    """避障逻辑"""

    # 无人车前往收货点
    res = client.set_tactical_maneuver(vehicle_id=vehicle1_id,position=[delivery],tactical_type=1,speed=100)

    # 根据get_command_status接口来判断是否到达收货点
    ret = 1
    while ret:
        try:
            # 获取res这个机动命令的状态
            status = client.get_command_status(res)
            # print(status)
            if status.get('status') == 'EXECUTE_SUCCESS':
                ret = 0
                break
        except Exception: 
            continue
    
    # 根据get_vehicle_pos获取无人车的位置信息
    res = client.get_vehicle_pos()
    for id,pos in res.items():
        if id == vehicle1_id:
            print(f"无人车{id}当前位置：{pos} ({pos.x}, {pos.y}, {pos.z})")

    # 与区域位置判断，判断无人车是否驶入该范围
    """
    判断逻辑
    """

    time.sleep(3)
    print("任务完成")
    
    pass